#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"

using Patrol = chapt4_interfaces::srv::Patrol; // 重定义
using SetParametersResult = rcl_interfaces::msg::SetParametersResult;   // 重定义

class TurtleControlNode : public rclcpp::Node
{
private:
    OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
    rclcpp::Service<Patrol>::SharedPtr patrol_service_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_; // 发布者的智能指针
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscriber_;  // 订阅者的智能指针
    double target_x_{9.0};
    double target_y_{9.0};
    double k_{1.0};         // 比例系数
    double max_speed_{3.0}; // 最大速度

public:
    explicit TurtleControlNode(const std::string &node_name) : Node(node_name)
    {
        // 声明和获取参数值
        this->declare_parameter("k", 1.0);
        this->declare_parameter("max_speed", 1.0);
        this->get_parameter("k", k_);
        this->get_parameter("max_speed", max_speed_);
        // this->set_parameter(rclcpp::Parameter("k", 2.0));   // 在代码中改变参数的值
        parameter_callback_handle_ = this->add_on_set_parameters_callback([&](const std::vector<rclcpp::Parameter> &parameters)->rcl_interfaces::msg::SetParametersResult
        {
            rcl_interfaces::msg::SetParametersResult result;
            result.successful = true;
            for(const auto& parameter : parameters)
            {
                RCLCPP_INFO(this->get_logger(), "更新参数值：%s = %f", parameter.get_name().c_str(), parameter.as_double());
                if("k" == parameter.get_name())
                    k_ = parameter.as_double();
                if("max_speed" == parameter.get_name())
                    max_speed_ = parameter.as_double();
            }
            return result;
        });
        patrol_service_ = this->create_service<Patrol>("patrol", [&](const Patrol::Request::SharedPtr request, Patrol::Response::SharedPtr response) -> void
        {
            if((0 < request->target_x && request->target_x < 12.0f) && (0 < request->target_y && request->target_y < 12.0f))
            {
                this->target_x_ = request->target_x;
                this->target_y_ = request->target_y;
                response->result = Patrol::Response::SUCESS;
            }
            else
            {
                response->result = Patrol::Response::FAIL;
            } 
        });
        publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);
        subscriber_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose", 10, std::bind(&TurtleControlNode::on_pose_received_, this, std::placeholders::_1));
    }

    void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose) // 参数：收到数据的共享指针
    {
        //  1.获取当前的位置
        auto current_x = pose->x;
        auto current_y = pose->y;
        // RCLCPP_INFO(get_logger(), "当前的位置：x=%f, y=%f", current_x, current_y); // 打印出来

        //  2.计算当前位置跟目标位置之间的距离差和角度差
        auto distance = std::sqrt((target_x_ - current_x) * (target_x_ - current_x) + (target_y_ - current_y) * (target_y_ - current_y)); // 距离差
        auto angle = std::atan2((target_y_ - current_y), (target_x_ - current_x)) /* 计算出目标点相对于当前点的角度(二维) */ - pose->theta;
        // RCLCPP_INFO(get_logger(), "距离差：%f, 角度差：%f", distance, angle);

        //  3.控制策略
        auto msg = geometry_msgs::msg::Twist();

        if (distance > 0.1)
        {
            if (fabs(angle) > 0.2)
            {
                msg.angular.z = fabs(angle); // 如果角度偏差过大就原地旋转
            }
            else
            {
                msg.linear.x = k_ * distance;
            }
        }

        //  4.限制线速度最大值
        if (msg.linear.x > max_speed_)
        {
            msg.linear.x = max_speed_;
        }

        publisher_->publish(msg);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TurtleControlNode>("turtle_control");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}